#include "RTDEClient.h"
#include "TcpClient.h"
#include "RTDEUtility.h"
#include "RTDEProtocol.h"

#define RTDE_PROTOCOL_VERSION 2

namespace CURCONTROL {

RTDEClient::RTDEClient():
    conn_state_(ConnectionState::DISCONNECTED),
    recipeSetupState_(false),
    frequency_(125),
    sendEnable_(true)
{

}

RTDEClient::~RTDEClient()
{
    sendEnable_ = false;
    conn_state_ = ConnectionState::DISCONNECTED;
    recipeSetupState_.store(false);
}

bool RTDEClient::Reconnect()
{
    if(BaseClient::Reconnect()){
        conn_state_ = ConnectionState::CONNECTED;
        return true;
    }
    return false;
}

bool RTDEClient::Connect(const std::string &robotIp)
{
    if(BaseClient::Connect(robotIp, RTDE_PORT, false)){
        conn_state_ = ConnectionState::CONNECTED;
        return true;
    }
    return false;
}

bool RTDEClient::IsStarted() const
{
    return conn_state_ == ConnectionState::STARTED;
}

void RTDEClient::Send(const RTDEProtocol &robot_cmd)
{
    if(!sendEnable_){
        return;
    }
//    std::cout<<"RTDE::CMD="<<robot_cmd.type_<<std::endl;
    std::uint8_t command = RTDECommand::RTDE_DATA_PACKAGE;
    std::vector<uint8_t> cmd_packed;
    cmd_packed = RTDEUtility::packInt32(robot_cmd.type_);

    if (robot_cmd.type_ == RTDEProtocol::FT_RTDE_INPUT_ENABLE ||
            robot_cmd.type_ == RTDEProtocol::ENABLE_EXTERNAL_FT_SENSOR)
    {
        std::vector<uint8_t> ft_rtde_input_enable_packed = RTDEUtility::packInt32(robot_cmd.ft_rtde_input_enable_);
        cmd_packed.insert(cmd_packed.end(), std::make_move_iterator(ft_rtde_input_enable_packed.begin()),
                          std::make_move_iterator(ft_rtde_input_enable_packed.end()));
    }

    if (robot_cmd.type_ == RTDEProtocol::SET_INPUT_INT_REGISTER)
    {
        std::vector<uint8_t> reg_int_packed = RTDEUtility::packInt32(robot_cmd.reg_int_val_);
        cmd_packed.insert(cmd_packed.end(), std::make_move_iterator(reg_int_packed.begin()),
                          std::make_move_iterator(reg_int_packed.end()));
    }

    if (robot_cmd.type_ == RTDEProtocol::SERVO_STOP)
    {
        std::vector<uint8_t> reg_double_packed = RTDEUtility::packDouble(robot_cmd.reg_double_val_);
        cmd_packed.insert(cmd_packed.end(), std::make_move_iterator(reg_double_packed.begin()),
                          std::make_move_iterator(reg_double_packed.end()));
    }

    if (robot_cmd.type_ == RTDEProtocol::SET_TOOL_VOLTAGE)
    {
        std::vector<uint8_t> voltage = RTDEUtility::packInt32(robot_cmd.reg_int_val_);
        cmd_packed.insert(cmd_packed.end(), std::make_move_iterator(voltage.begin()),
                          std::make_move_iterator(voltage.end()));
    }

    if (robot_cmd.type_ == RTDEProtocol::FORCE_MODE)
    {
        std::vector<uint8_t> force_mode_type_packed = RTDEUtility::packInt32(robot_cmd.force_mode_type_);
        cmd_packed.insert(cmd_packed.end(), std::make_move_iterator(force_mode_type_packed.begin()),
                          std::make_move_iterator(force_mode_type_packed.end()));

        std::vector<uint8_t> sel_vector_packed = RTDEUtility::packVectorNInt32(robot_cmd.selection_vector_);
        cmd_packed.insert(cmd_packed.end(), std::make_move_iterator(sel_vector_packed.begin()),
                          std::make_move_iterator(sel_vector_packed.end()));
    }

    if (robot_cmd.type_ == RTDEProtocol::GET_ACTUAL_JOINT_POSITIONS_HISTORY)
    {
        std::vector<uint8_t> actual_joint_positions_history_packed = RTDEUtility::packUInt32(robot_cmd.steps_);
        cmd_packed.insert(cmd_packed.end(), std::make_move_iterator(actual_joint_positions_history_packed.begin()),
                          std::make_move_iterator(actual_joint_positions_history_packed.end()));
    }

    if (!robot_cmd.val_.empty())
    {
        std::vector<uint8_t> vector_nd_packed = RTDEUtility::packVectorNd(robot_cmd.val_);
        cmd_packed.insert(cmd_packed.end(), std::make_move_iterator(vector_nd_packed.begin()),
                          std::make_move_iterator(vector_nd_packed.end()));
    }

    if (robot_cmd.type_ == RTDEProtocol::MOVEJ || robot_cmd.type_ == RTDEProtocol::MOVEJ_IK ||
            robot_cmd.type_ == RTDEProtocol::MOVEL || robot_cmd.type_ == RTDEProtocol::MOVEL_FK ||
            robot_cmd.type_ == RTDEProtocol::MOVE_PATH || robot_cmd.type_ == RTDEProtocol::STOPJ ||
            robot_cmd.type_ == RTDEProtocol::STOPL)
    {
        std::vector<uint8_t> async_packed = RTDEUtility::packInt32(robot_cmd.async_);
        cmd_packed.insert(cmd_packed.end(), std::make_move_iterator(async_packed.begin()),
                          std::make_move_iterator(async_packed.end()));
    }

    if (robot_cmd.type_ == RTDEProtocol::SET_STD_DIGITAL_OUT)
    {
        cmd_packed.push_back(robot_cmd.std_digital_out_mask_);
        cmd_packed.push_back(robot_cmd.std_digital_out_);
    }

    if (robot_cmd.type_ == RTDEProtocol::SET_CONF_DIGITAL_OUT)
    {
        cmd_packed.push_back(robot_cmd.configurable_digital_out_mask_);
        cmd_packed.push_back(robot_cmd.configurable_digital_out_);
    }

    if (robot_cmd.type_ == RTDEProtocol::SET_TOOL_DIGITAL_OUT)
    {
        cmd_packed.push_back(robot_cmd.std_tool_out_mask_);
        cmd_packed.push_back(robot_cmd.std_tool_out_);
    }

    if (robot_cmd.type_ == RTDEProtocol::SET_SPEED_SLIDER)
    {
        std::vector<uint8_t> speed_slider_mask_packed = RTDEUtility::packInt32(robot_cmd.speed_slider_mask_);
        cmd_packed.insert(cmd_packed.end(), std::make_move_iterator(speed_slider_mask_packed.begin()),
                          std::make_move_iterator(speed_slider_mask_packed.end()));

        std::vector<uint8_t> speed_slider_fraction_packed = RTDEUtility::packDouble(robot_cmd.speed_slider_fraction_);
        cmd_packed.insert(cmd_packed.end(), std::make_move_iterator(speed_slider_fraction_packed.begin()),
                          std::make_move_iterator(speed_slider_fraction_packed.end()));
    }

    if (robot_cmd.type_ == RTDEProtocol::SET_STD_ANALOG_OUT)
    {
        cmd_packed.push_back(robot_cmd.std_analog_output_mask_);
        cmd_packed.push_back(robot_cmd.std_analog_output_type_);
        std::vector<uint8_t> std_analog_output_0_packed = RTDEUtility::packDouble(robot_cmd.std_analog_output_0_);
        cmd_packed.insert(cmd_packed.end(), std::make_move_iterator(std_analog_output_0_packed.begin()),
                          std::make_move_iterator(std_analog_output_0_packed.end()));
        std::vector<uint8_t> std_analog_output_1_packed = RTDEUtility::packDouble(robot_cmd.std_analog_output_1_);
        cmd_packed.insert(cmd_packed.end(), std::make_move_iterator(std_analog_output_1_packed.begin()),
                          std::make_move_iterator(std_analog_output_1_packed.end()));
    }

    cmd_packed.insert(cmd_packed.begin(), robot_cmd.recipe_id_);
    std::string sent(cmd_packed.begin(), cmd_packed.end());

    SendAll(command, sent);
}

void RTDEClient::Run()
{
    if(this->conn_state_ == ConnectionState::CONNECTED){
        this->NegotiateProtocolVersion();
        this->GetControllerVersion();
        if(!recipeSetupState_.load()){// 未注册状态
            this->SetupRecipes();
        }
        this->SendStart(); // 开始RTDE数据同步
        std::chrono::high_resolution_clock::time_point start_time =
                std::chrono::high_resolution_clock::now();
        while (!this->IsStarted()) {
            // Wait until RTDE data synchronization has started or timeout
            std::chrono::high_resolution_clock::time_point current_time =
                    std::chrono::high_resolution_clock::now();
            auto duration = std::chrono::duration_cast<std::chrono::seconds>(
                        current_time - start_time)
                    .count();
            if (duration > 10) {
                break;
            }
        }
        if (!this->IsStarted()){
            std::string error_str{"Failed to start RTDE data synchronization, before timeout"};
            throw std::logic_error(error_str);
        }
//        printf("Successed to start RTDE data synchronization\n");
    }
}

void RTDEClient::SetSendEnable(bool enable)
{
    this->sendEnable_ = enable;
}

//const std::vector<std::string> &RTDEClient::GetOutputNames() const
//{
//    return this->output_names_;
//}

void RTDEClient::Clear()
{
    this->conn_state_ = ConnectionState::DISCONNECTED;
    recipeSetupState_.store(false);
}

void RTDEClient::SendStart()
{
    uint8_t cmd = RTDE_CONTROL_PACKAGE_START;
    SendAll(cmd, "");
    Receive();
}

void RTDEClient::SetupRecipes()
{
    // Setup output
    std::vector<std::string> state_names = RTDEProtocol::GetOutputNames();

    this->SendOutputSetup(state_names);

    // Setup input recipes
    // Recipe 1
    std::vector<std::string> async_setp_input = {
        RTDEUtility::InIntReg(0),    RTDEUtility::InDoubleReg(0), RTDEUtility::InDoubleReg(1), RTDEUtility::InDoubleReg(2),
        RTDEUtility::InDoubleReg(3), RTDEUtility::InDoubleReg(4), RTDEUtility::InDoubleReg(5), RTDEUtility::InDoubleReg(6),
        RTDEUtility::InDoubleReg(7), RTDEUtility::InIntReg(1)};
    this->SendInputSetup(async_setp_input);

    // Recipe 2
    std::vector<std::string> servoj_input = {
        RTDEUtility::InIntReg(0),    RTDEUtility::InDoubleReg(0), RTDEUtility::InDoubleReg(1), RTDEUtility::InDoubleReg(2),
        RTDEUtility::InDoubleReg(3), RTDEUtility::InDoubleReg(4), RTDEUtility::InDoubleReg(5), RTDEUtility::InDoubleReg(6),
        RTDEUtility::InDoubleReg(7), RTDEUtility::InDoubleReg(8), RTDEUtility::InDoubleReg(9), RTDEUtility::InDoubleReg(10)};
    this->SendInputSetup(servoj_input);

    // Recipe 3
    std::vector<std::string> force_mode_input = {
        RTDEUtility::InIntReg(0),     RTDEUtility::InIntReg(1),     RTDEUtility::InIntReg(2),     RTDEUtility::InIntReg(3),
        RTDEUtility::InIntReg(4),     RTDEUtility::InIntReg(5),     RTDEUtility::InIntReg(6),     RTDEUtility::InIntReg(7),
        RTDEUtility::InDoubleReg(0),  RTDEUtility::InDoubleReg(1),  RTDEUtility::InDoubleReg(2),  RTDEUtility::InDoubleReg(3),
        RTDEUtility::InDoubleReg(4),  RTDEUtility::InDoubleReg(5),  RTDEUtility::InDoubleReg(6),  RTDEUtility::InDoubleReg(7),
        RTDEUtility::InDoubleReg(8),  RTDEUtility::InDoubleReg(9),  RTDEUtility::InDoubleReg(10), RTDEUtility::InDoubleReg(11),
        RTDEUtility::InDoubleReg(12), RTDEUtility::InDoubleReg(13), RTDEUtility::InDoubleReg(14), RTDEUtility::InDoubleReg(15),
        RTDEUtility::InDoubleReg(16), RTDEUtility::InDoubleReg(17)};
    this->SendInputSetup(force_mode_input);

    // Recipe 4
    std::vector<std::string> no_cmd_input = {RTDEUtility::InIntReg(0)};
    this->SendInputSetup(no_cmd_input);

    // Recipe 5
    std::vector<std::string> servoc_input = {
        RTDEUtility::InIntReg(0),    RTDEUtility::InDoubleReg(0), RTDEUtility::InDoubleReg(1), RTDEUtility::InDoubleReg(2),
        RTDEUtility::InDoubleReg(3), RTDEUtility::InDoubleReg(4), RTDEUtility::InDoubleReg(5), RTDEUtility::InDoubleReg(6),
        RTDEUtility::InDoubleReg(7), RTDEUtility::InDoubleReg(8)};
    this->SendInputSetup(servoc_input);

    // Recipe 6
    std::vector<std::string> wrench_input = {
        RTDEUtility::InIntReg(0),    RTDEUtility::InDoubleReg(0), RTDEUtility::InDoubleReg(1), RTDEUtility::InDoubleReg(2),
        RTDEUtility::InDoubleReg(3), RTDEUtility::InDoubleReg(4), RTDEUtility::InDoubleReg(5)};
    this->SendInputSetup(wrench_input);

    // Recipe 7
    std::vector<std::string> set_payload_input = {RTDEUtility::InIntReg(0), RTDEUtility::InDoubleReg(0),
                                                  RTDEUtility::InDoubleReg(1), RTDEUtility::InDoubleReg(2),
                                                  RTDEUtility::InDoubleReg(3)};
    this->SendInputSetup(set_payload_input);

    // Recipe 8
    std::vector<std::string> force_mode_parameters_input = {RTDEUtility::InIntReg(0),
                                                            RTDEUtility::InDoubleReg(0)};
    this->SendInputSetup(force_mode_parameters_input);

    // Recipe 9
    std::vector<std::string> get_actual_joint_positions_history_input = {
        RTDEUtility::InIntReg(0), RTDEUtility::InIntReg(1)};
    this->SendInputSetup(get_actual_joint_positions_history_input);

    // Recipe 10
    std::vector<std::string> get_inverse_kin_input = {
        RTDEUtility::InIntReg(0),     RTDEUtility::InDoubleReg(0),  RTDEUtility::InDoubleReg(1), RTDEUtility::InDoubleReg(2),
        RTDEUtility::InDoubleReg(3),  RTDEUtility::InDoubleReg(4),  RTDEUtility::InDoubleReg(5), RTDEUtility::InDoubleReg(6),
        RTDEUtility::InDoubleReg(7),  RTDEUtility::InDoubleReg(8),  RTDEUtility::InDoubleReg(9), RTDEUtility::InDoubleReg(10),
        RTDEUtility::InDoubleReg(11), RTDEUtility::InDoubleReg(12), RTDEUtility::InDoubleReg(13)};
    this->SendInputSetup(get_inverse_kin_input);

    // Recipe 11
    std::vector<std::string> watchdog_input = {RTDEUtility::InIntReg(0)};
    this->SendInputSetup(watchdog_input);

    // Recipe 12
    std::vector<std::string> pose_trans_input = {
        RTDEUtility::InIntReg(0),    RTDEUtility::InDoubleReg(0), RTDEUtility::InDoubleReg(1), RTDEUtility::InDoubleReg(2),
        RTDEUtility::InDoubleReg(3), RTDEUtility::InDoubleReg(4), RTDEUtility::InDoubleReg(5), RTDEUtility::InDoubleReg(6),
        RTDEUtility::InDoubleReg(7), RTDEUtility::InDoubleReg(8), RTDEUtility::InDoubleReg(9), RTDEUtility::InDoubleReg(10),
        RTDEUtility::InDoubleReg(11)};
    this->SendInputSetup(pose_trans_input);

    // Recipe 13
    std::vector<std::string> setp_input = {
        RTDEUtility::InIntReg(0),    RTDEUtility::InDoubleReg(0), RTDEUtility::InDoubleReg(1),
        RTDEUtility::InDoubleReg(2), RTDEUtility::InDoubleReg(3), RTDEUtility::InDoubleReg(4),
        RTDEUtility::InDoubleReg(5), RTDEUtility::InDoubleReg(6), RTDEUtility::InDoubleReg(7)};
    this->SendInputSetup(setp_input);

    // Recipe 14
    std::vector<std::string> jog_input = {
        RTDEUtility::InIntReg(0),     RTDEUtility::InDoubleReg(0), RTDEUtility::InDoubleReg(1), RTDEUtility::InDoubleReg(2),
        RTDEUtility::InDoubleReg(3),  RTDEUtility::InDoubleReg(4), RTDEUtility::InDoubleReg(5), RTDEUtility::InDoubleReg(6),
        RTDEUtility::InDoubleReg(7),  RTDEUtility::InDoubleReg(8), RTDEUtility::InDoubleReg(9), RTDEUtility::InDoubleReg(10),
        RTDEUtility::InDoubleReg(11), RTDEUtility::InDoubleReg(12)};
    this->SendInputSetup(jog_input);

    // Recipe 15
    std::vector<std::string> async_path_input = {RTDEUtility::InIntReg(0), RTDEUtility::InIntReg(1)};
    this->SendInputSetup(async_path_input);

    // Recipe 16
    std::vector<std::string> move_until_contact_input = {
        RTDEUtility::InIntReg(0),     RTDEUtility::InDoubleReg(0), RTDEUtility::InDoubleReg(1), RTDEUtility::InDoubleReg(2),
        RTDEUtility::InDoubleReg(3),  RTDEUtility::InDoubleReg(4), RTDEUtility::InDoubleReg(5), RTDEUtility::InDoubleReg(6),
        RTDEUtility::InDoubleReg(7),  RTDEUtility::InDoubleReg(8), RTDEUtility::InDoubleReg(9), RTDEUtility::InDoubleReg(10),
        RTDEUtility::InDoubleReg(11), RTDEUtility::InDoubleReg(12)};
    this->SendInputSetup(move_until_contact_input);

    // Recipe 17
    std::vector<std::string> freedrive_mode_input = {
        RTDEUtility::InIntReg(0),    RTDEUtility::InIntReg(1),    RTDEUtility::InIntReg(2),    RTDEUtility::InIntReg(3),
        RTDEUtility::InIntReg(4),    RTDEUtility::InIntReg(5),    RTDEUtility::InIntReg(6),    RTDEUtility::InDoubleReg(0),
        RTDEUtility::InDoubleReg(1), RTDEUtility::InDoubleReg(2), RTDEUtility::InDoubleReg(3), RTDEUtility::InDoubleReg(4),
        RTDEUtility::InDoubleReg(5)};
    this->SendInputSetup(freedrive_mode_input);

    // Recipe 18
    std::vector<std::string> external_ft_input = {RTDEUtility::InIntReg(0),
                                                  "external_force_torque"};
    this->SendInputSetup(external_ft_input);

    // Recipe 19
    std::vector<std::string> ft_thisinput_enable = {
        RTDEUtility::InIntReg(0),    RTDEUtility::InIntReg(1),    RTDEUtility::InDoubleReg(0),
        RTDEUtility::InDoubleReg(1), RTDEUtility::InDoubleReg(2), RTDEUtility::InDoubleReg(3),
        RTDEUtility::InDoubleReg(4), RTDEUtility::InDoubleReg(5), RTDEUtility::InDoubleReg(6)};
    this->SendInputSetup(ft_thisinput_enable);

    // Recipe 20 - STOPL and STOPJ
    std::vector<std::string> stopl_stopj_input = {RTDEUtility::InIntReg(0), RTDEUtility::InDoubleReg(0),
                                                  RTDEUtility::InIntReg(1)};
    this->SendInputSetup(stopl_stopj_input);
    recipeSetupState_.store(true);
    std::string str{"注册输出配方项"};
    RTDEUtility::PrintStr(str);
}

void RTDEClient::SendOutputSetup(const std::vector<std::string> &outNames)
{
//    this->output_names_ = outNames;
    std::uint8_t cmd = RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS;
    std::string freq_as_hexstr = RTDEUtility::double2hexstr(frequency_);
    std::vector<uint8_t> freq_packed = RTDEUtility::hexToBytes(freq_as_hexstr);
    // Concatenate output_names to a single string
    std::string output_names_str;
    for (const auto &ele : outNames){
        output_names_str += ele + ",";
    }

    std::copy(output_names_str.begin(), output_names_str.end(),
              std::back_inserter(freq_packed));
    std::string payload(std::begin(freq_packed), std::end(freq_packed));
    SendAll(cmd, payload);
    Receive();
}

void RTDEClient::SendInputSetup(const std::vector<std::string> &inNames)
{
    std::uint8_t cmd = RTDE_CONTROL_PACKAGE_SETUP_INPUTS;
    // Concatenate input_names to a single string
    std::string input_names_str;
    for (const auto &ele : inNames)
        input_names_str += ele + ",";
    SendAll(cmd, input_names_str);
    Receive();
}

void RTDEClient::GetControllerVersion()
{
    std::uint8_t cmd = RTDE_GET_URCONTROL_VERSION;
    RTDEUtility::PrintStr("GetControllerVersion");
    SendAll(cmd, "");
    Receive();
}

void RTDEClient::NegotiateProtocolVersion()
{
    std::uint8_t cmd = RTDE_REQUEST_PROTOCOL_VERSION;
    // Pack RTDE_PROTOCOL_VERSION into payload
    uint8_t null_byte = 0;
    uint8_t version = RTDE_PROTOCOL_VERSION;
    std::vector<uint8_t> buffer;
    buffer.push_back(null_byte);
    buffer.push_back(version);
    std::string payload(buffer.begin(), buffer.end());
    RTDEUtility::PrintStr("NegotiateProtocolVersion");
    SendAll(cmd, payload);
    Receive();
}

void RTDEClient::SendAll(uint8_t command, const std::string &payload)
{
    // Pack size and command into header
    uint16_t size = htons(RTDEProtocol::HEADER_SIZE + (uint16_t)payload.size());
    uint8_t type = command;

    char buffer[3];
    memcpy(buffer + 0, &size, sizeof(size));
    memcpy(buffer + 2, &type, sizeof(type));

    // Create vector<char> that includes the header
    std::vector<char> header_packed;
    std::copy(buffer, buffer + sizeof(buffer), std::back_inserter(header_packed));

    // Add the payload to the header_packed vector
    std::copy(payload.begin(), payload.end(), std::back_inserter(header_packed));

    std::string sent(header_packed.begin(), header_packed.end());

    // This is a workaround for the moment to prevent crash when calling this
    // function is RTDE is disconnected - i.e. in case of desynchronization
    if (IsConnected())
    {
        if(this->tcpClient_->Write(sent)){

        }else{
            printf("send failed\n");
        }
    }
}

void RTDEClient::Receive()
{
    // Read 报头
    std::vector<uint8_t> data(RTDEProtocol::HEADER_SIZE);
    if (IsConnected())
        this->tcpClient_->Read(data);
    uint32_t message_offset = 0;
    uint16_t msg_size = RTDEUtility::getUInt16(data, message_offset);
    uint8_t msg_cmd = static_cast<uint8_t>(data.at(2));
    // Read 负载数据
    data.resize(msg_size - RTDEProtocol::HEADER_SIZE);
    if (IsConnected())
        this->tcpClient_->Read(data);

    switch (msg_cmd) {
    case RTDE_TEXT_MESSAGE: {
        uint8_t msg_length = static_cast<uint8_t>(data.at(0));
        std::string msg;
        for (unsigned long i = 1; i < msg_length; i++) {
            msg.push_back(data[i]);
        }
        printf("RTDE_TEXT_MESSAGE:\n %s\n", msg.c_str());
        break;
    }

    case RTDE_REQUEST_PROTOCOL_VERSION: {
        uint8_t success = data.at(0);
        auto thissuccess = static_cast<bool>(success);
        if (thissuccess) {
            std::string str{"RTDE_REQUEST_PROTOCOL_VERSION 成功"};
            RTDEUtility::PrintStr(str);
        } else{
            std::string str{"RTDE_REQUEST_PROTOCOL_VERSION 失败"};
            RTDEUtility::PrintStr(str);
        }
        break;
    }

    case RTDE_GET_URCONTROL_VERSION: {
        std::uint32_t message_offset = 0;
        std::uint32_t v_major = RTDEUtility::getUInt32(data, message_offset);
        std::uint32_t v_minor = RTDEUtility::getUInt32(data, message_offset);
        std::uint32_t v_bugfix = RTDEUtility::getUInt32(data, message_offset);
        std::uint32_t v_build = RTDEUtility::getUInt32(data, message_offset);
        this->versions_ = std::make_tuple(v_major, v_minor, v_bugfix, v_build);
        const uint32_t CB3_MAJOR_VERSION{3};
        if(v_major > CB3_MAJOR_VERSION){
            this->frequency_ = 500;
        }
        std::string str{"version "};
        str.append(std::to_string(v_major));
        str.append(".");
        str.append(std::to_string(v_minor));
        str.append(".");
        str.append(std::to_string(v_bugfix));
        str.append(".");
        str.append(std::to_string(v_build));
        RTDEUtility::PrintStr(str);
        break;
    }

    case RTDE_CONTROL_PACKAGE_SETUP_INPUTS: {
        std::string datatypes(std::begin(data) + 1, std::end(data));
        std::string in_use_str("IN_USE");
        if (datatypes.find(in_use_str) != std::string::npos) {
            std::string error_str{"One of the RTDE input registers are already in "
                                  "use! Currently you must disable the "
                                  "EtherNet/IP adapter, "
                                  "PROFINET or any MODBUS unit configured on the "
                                  "robot. This might change in the future."};
            RTDEUtility::PrintStr(datatypes);
            throw std::runtime_error(error_str);
        }
        break;
    }

    case RTDE_CONTROL_PACKAGE_SETUP_OUTPUTS: {
        // char id = data.at(0);
        // DEBUG("ID:" << id);
        std::string datatypes(std::begin(data) + 1, std::end(data));
        std::vector<std::string> output_types;
        output_types = RTDEUtility::split(datatypes, ',');

        std::string not_found_str("NOT_FOUND");
        std::vector<unsigned long> not_found_indexes;
        if (datatypes.find(not_found_str) != std::string::npos) {
            for (unsigned int i = 0; i < output_types.size(); i++) {
                if (output_types[i] == "NOT_FOUND")
                    not_found_indexes.push_back(i);
            }

            std::string vars_not_found;
            for (unsigned int i = 0; i < not_found_indexes.size(); i++) {
                vars_not_found += OUTPUT_VARIABLES[not_found_indexes[i]].name_;
                if (i != not_found_indexes.size() - 1)
                    vars_not_found += ", ";
            }

            std::string error_str(
                        "The following variables was not found by the controller: [" +
                        vars_not_found + "],\n see which variables are supported by your "
                                         "PolyScope version here: \n"
                                         "https://www.universal-robots.com/articles/ur/"
                                         "interface-communication/"
                                         "real-time-data-exchange-rtde-guide/");
            throw std::runtime_error(error_str);
        }
        break;
    }

    case RTDE_CONTROL_PACKAGE_START: {
        char success = data.at(0);
        auto thissuccess = static_cast<bool>(success);
        if (thissuccess) {
            conn_state_ = ConnectionState::STARTED;
            std::string str{"RTDE synchronization started"};
            RTDEUtility::PrintStr(str);
        } else{
            std::string str{"Unable to start synchronization"};
            RTDEUtility::PrintStr(str);
        }
        break;
    }

    case RTDE_CONTROL_PACKAGE_PAUSE: {
        char success = data.at(0);
        auto pause_success = static_cast<bool>(success);
        if (pause_success) {
            conn_state_ = ConnectionState::PAUSED;
        } else
            std::cerr << "Unable to pause synchronization" << std::endl;
        break;
    }

    default:
        break;
    }
}

}// namespace CURCONTROL
